#include "length_diff.h"

bool length_diff(MPolygon *apoly,
                 std::vector<Vector2d> &increments)
{
    if(!apoly) return false;
    int last =  apoly->points.size();
    increments.resize(last, Vector2d());
    for(int i = 0; i < last;i++){

        Point2d p1(apoly->points[(i-2 + last)%last].x,
                   apoly->points[(i-2 + last)%last].y);
        //p(i-1)
        Point2d p2(apoly->points[(i-1 + last)%last].x,
                   apoly->points[(i-1 + last)%last].y);
        //p(i)
        Point2d p3(apoly->points[(i-0 + last)%last].x,
                   apoly->points[(i-0 + last)%last].y);
        //p(i+1)
        Point2d p4(apoly->points[(i+1 + last)%last].x,
                   apoly->points[(i+1 + last)%last].y);
        //p(i+2)
        Point2d p5(apoly->points[(i+2 + last)%last].x,
                   apoly->points[(i+2 + last)%last].y);

        Vector2d e1(p1, p2);
        Vector2d e2(p2, p3);
        Vector2d e3(p3, p4);
        Vector2d e4(p4, p5);

        increments[i] =-2.0*(e4.length() - e3.length())*e3/e3.length()+
                        2.0*(e3.length() - e2.length())*(e3/e3.length()-e2/e2.length())+
                        2.0*(e2.length() - e1.length())*e2/e2.length();
    }
}
